#include "LdPcl.h"
#include "odometry.h"
int main()
{
    std::string path2;
    char temp[100];
    PcloudI tempCloud(new CloudI());
    for(size_t i = 0; i < 105; i++)
    {
        // sprintf(temp, "/home/milo/Pictures/pcd/222test222_%d.pcd", i);
        sprintf(temp, "/home/milo/Pictures/pcd/test1_%d.pcd", i);
        path2 = temp;
        pcl::io::loadPCDFile(path2, *tempCloud);
        pcl::visualization::CloudViewer V_2("Viewer");
        V_2.showCloud(tempCloud);
        while(!V_2.wasStopped()){}
    }
    // Gyodometry &odometry = Gyodometry::GetInstance();
    Gyodometry &odometry = Gyodometry::GetInstance();
    // odometry.DataCL();
    std::vector<std::string> BroadCode;
    BroadCode.push_back("000000000000001");
    LdsLidar &mylidar = LdsLidar::GetInstance();
    int init = mylidar.InitLdsLidar(BroadCode, 20000);
    std::string saved_path;
    sleep(0.5);
    while (init == 0)
    {
        init = mylidar.InitLdsLidar(BroadCode, 20000);
        sleep(1);
    }
    // std::thread Th_dataCL(&Gyodometry::DataCL, &odometry);
    // Th_dataCL.join();
    sleep(2);
    PcloudI AllClouds(new CloudI());
    Gydata OriginData;
    Gydata TargetData;
    std::vector<PcloudI> cloudIs;
    int count = 0;
    std::vector<Gydata> Gydatas;
    char path[100];
    while (true)
    {
        sprintf(path, "/home/milo/Pictures/pcd/222test222_%d.pcd", count);
        saved_path = path;
        PcloudI TempCloud(new CloudI());
        mylidar.getcloud(TempCloud);
        sleep(1);
        while (TempCloud == nullptr || TempCloud->size() < 15000)
        {
            sleep(0.01);
            // OriginCloud = mylidar.GetPointCloud();
            // OriginCloud = mylidar.GetPointCloud_I();
            mylidar.getcloud(TempCloud);
        }
        TempCloud->height = 1;
        TempCloud->width = TempCloud->points.size();
        TempCloud->is_dense = false;
        pcl::io::savePCDFile(saved_path, *TempCloud);
        // pcl::visualization::CloudViewer V_every("Viewer Every");
        // V_every.showCloud(TempCloud);
        // while (!V_every.wasStopped())
        // {
        // }
        // sleep(1);
        // Gydata temp;
        // odometry.DataCB(temp);
        // Gydatas.emplace_back(temp);
        // cloudIs.emplace_back(TempCloud);
        // std::cout << "Gydatas yaw: " << Gydatas[count].yaw << "  Tran: " << Gydatas[count].Tran << std::endl;
        // std::cout << "Gydatas Last: " << Gydatas[count-1].yaw << std::endl;
        count++;
        if (count % 1000 != 0)
            continue;
        count = 0;
        //开始进行点云配准
        for (int i = 0; i < Gydatas.size(); i++)
            std::cout << Gydatas[i].yaw << std::endl;

        std::cout << "the size of vector is: " << Gydatas.size() << std::endl;
        // *AllClouds = *(cloudIs[0]);
        std::cout << "the 0 yaw is: " << Gydatas[count].yaw << std::endl;
        for (int i = 1; i < 5; i++)
        {
            Eigen::Matrix4f RoT;
            float yaw = (Gydatas[i].yaw - Gydatas[0].yaw) * CV_PI / 180;
            // float yaw = Gydatas[i].yaw;
            std::cout << "yaw is: " << yaw << std::endl;
            Eigen::Vector3f tTran = Gydatas[i].Tran - Gydatas[0].Tran;
            std::cout << "--- tTran is: ---" << tTran << std::endl;
            RoT << cos(yaw), -sin(yaw), 0, 0,
                sin(yaw), cos(yaw), 0, tTran.y(),
                0, 0, 1, tTran.z(),
                0, 0, 0, 1;
            // RoT << 1, 0, 0, 0,
            //     0, 1, 0, 0,
            //     0, 0, 1, 0,
            //     0, 0, 0, 1;

            Fusion(cloudIs[i], AllClouds, RoT);
        }
        count = 0;
        pcl::visualization::CloudViewer V_All("Viewer AllcloudIs");
        std::cout << "the points is: " << AllClouds->size() << std::endl;
        V_All.showCloud(AllClouds);
        while (!V_All.wasStopped())
        {
        }
        Gydatas.clear();
        cloudIs.clear();
    }
}